// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ExecuteSingleLinearTouchAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Manipulation.Metrics
{
    using System;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Executes a single linear touch motion based.
    /// </summary>
    [DataContract]
    public class ExecuteSingleLinearTouchAgent : ConsumerAgent<TimerMessage>, ISubscriptionReceiver<ManipulatorStatusMessage>
    {
        /// <summary>
        /// Position threshold for considering a motion done
        /// </summary>
        private const double positionDeltaThreshold = 0.01;

        /// <summary>
        /// Orientation threshold for considering a motion done.
        /// </summary>
        private const double orientationDeltaThreshold = 0.04;

        /// <summary>
        /// state of the agent.
        /// </summary>
        private States state;

        /// <summary>
        /// States that the agent can be in
        /// </summary>
        private enum States
        {
            /// <summary>
            /// Initial state
            /// </summary>
            NotStarted,

            /// <summary>
            /// State during motion to start position
            /// </summary>
            GoingToStartPosition,

            /// <summary>
            /// State during touch execution
            /// </summary>
            ExecutingTouch
        }
        /// <summary>
        /// initial pose
        /// </summary>
        [DataMember(Name = "StartPose")]
        private Pose startPose;

        /// <summary>
        /// pose to move towards during the touch operation
        /// </summary>
        [DataMember(Name = "TargetPose")]
        private Pose targetPose;

        /// <summary>
        /// force threshold for determing contact with unknown object during touch
        /// </summary>
        [DataMember(Name = "ForceThresholdInNewtons")]
        private double forceThrehsoldInNewtons;

        /// <summary>
        /// linear speed of travel
        /// </summary>
        [DataMember(Name = "LinearSpeedInMetersPerSecond")]
        private double linearSpeedInMetersPerSecond;

        /// <summary>
        /// rotational speed of travel
        /// </summary>
        [DataMember(Name = "RotationalSpeedInRadiansPerSecond")]
        private double rotationalSpeedInRadiansPerSecond;

        /// <summary>
        /// Name of agent to subscribe to producing manipulation status messages
        /// </summary>
        [DataMember(Name = "ManipulationAgentName")]
        private string manipulationAgentName;

 
        /// <summary>
        /// Agent to subscribe to producing manipulation status messages
        /// </summary>
        private IAgentHost manipulationAgent;

        /// <summary>
        /// Most recently recieved manipulation status message
        /// </summary>
        private ManipulatorStatusMessage latestManipulatorStatusMessage;

        /// <summary>
        /// Control port for tcp cartesian control
        /// </summary>
        private IForwarder<LinearCartesianMotionControlMessage> tcpCartesianMoveControlPort;

        /// <summary>
        /// Control port for tcp cartesian touch control
        /// </summary>
        private IForwarder<LinearCartesianTouchControlMessage> tcpCartesianTouchControlPort;

        /// <summary>
        /// Initializes a new instance of the <c>ExecuteSingleLinearTouchAgent</c> class.
        /// </summary>
        /// <param name="name"> Agent name. </param>
        /// <param name="producer"> Timer producer. </param>
        /// <param name="manipulationAgentName">Manipulation agent name.</param>
        /// <param name="startPose">Pose to start the touch motion from.</param>
        /// <param name="targetPose">Pose to move linear towards during touch motion.</param>
        /// <param name="forceThresholdInNewtons">Force threshold for detecting contact.</param>
        /// <param name="linearSpeedInMetersPerSecond">Linear speed to move set point at.</param>
        /// <param name="rotationalSpeedInRadiansPerSecond">Rotational speed to move setpoint at.</param>
       public ExecuteSingleLinearTouchAgent(
            string name,
            IProducer<TimerMessage> producer,
            string manipulationAgentName,
            Pose startPose,
            Pose targetPose,
            double forceThresholdInNewtons,
            double linearSpeedInMetersPerSecond,
            double rotationalSpeedInRadiansPerSecond)
            : base(name, producer.Name)
        {
            this.manipulationAgentName = manipulationAgentName;
            this.state = States.NotStarted;
            this.startPose = startPose;
            this.targetPose = targetPose;
            this.forceThrehsoldInNewtons = forceThresholdInNewtons;
            this.linearSpeedInMetersPerSecond = linearSpeedInMetersPerSecond;
            this.rotationalSpeedInRadiansPerSecond = rotationalSpeedInRadiansPerSecond;
        }

        /// <summary>
        /// Initializes the agent.
        /// </summary>
        /// <param name="locator">The agent locator.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            if (startPose == null)
            {
                throw new ArgumentException("StartPose cannot be null");
            }

            if (targetPose == null)
            {
                throw new ArgumentException("TargetPose cannot be null");
            }

            if (forceThrehsoldInNewtons <= 0)
            {
                throw new ArgumentException("ForceThresholdInNewtons must be positive");
            }

            if (linearSpeedInMetersPerSecond <= 0)
            {
                throw new ArgumentException("LinearSpeedInMetersPerSecond must be positive");
            }
 
            if (rotationalSpeedInRadiansPerSecond <= 0)
            {
                throw new ArgumentException("RotationalSpeedInRadiansPerSecond must be positive");
            }
 
            if(false == string.IsNullOrEmpty(this.manipulationAgentName))
            {
                this.manipulationAgent = locator.GetAgent(this.manipulationAgentName);
                this.manipulationAgent.Subscribe<ManipulatorStatusMessage>(this);
                this.tcpCartesianMoveControlPort = manipulationAgent.GetControlPort<LinearCartesianMotionControlMessage>();
                this.tcpCartesianTouchControlPort = manipulationAgent.GetControlPort<LinearCartesianTouchControlMessage>();
            }
        }

        /// <summary>
        /// Called when agent is activated
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
            this.manipulationAgent.Activate();
            this.state = States.NotStarted;
        }

        /// <summary>
        /// Called when agent is deactivated
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
            this.manipulationAgent.Deactivate();
        }

        /// <summary>
        /// Called when a timer message is recieved
        /// </summary>
        /// <param name="message"></param>
        public override void Receive(TimerMessage message)
        {
            switch (this.state)
            {
                case States.NotStarted:

                    LinearCartesianMotionControlMessage moveMsg = new LinearCartesianMotionControlMessage(
                        message.OriginatingTime,
                        this.startPose,
                        0);

                    this.tcpCartesianMoveControlPort.Post(moveMsg);

                    this.state = States.GoingToStartPosition;

                    break;

                case States.GoingToStartPosition:
                    if ((latestManipulatorStatusMessage.State.TcpCartesianPosition.Position - this.startPose.Position).Length() < positionDeltaThreshold &&
                        Math.Min(Quaternion.Length(latestManipulatorStatusMessage.State.TcpCartesianPosition.Orientation - this.startPose.Orientation),
                                 Quaternion.Length(latestManipulatorStatusMessage.State.TcpCartesianPosition.Orientation + this.startPose.Orientation))
                        < orientationDeltaThreshold)
                    {
                        LinearCartesianTouchControlMessage touchMsg = new LinearCartesianTouchControlMessage(
                            message.OriginatingTime,
                            this.targetPose,
                            this.linearSpeedInMetersPerSecond,
                            this.rotationalSpeedInRadiansPerSecond,
                            this.forceThrehsoldInNewtons);

                        this.tcpCartesianTouchControlPort.Post(touchMsg);

                        this.state = States.ExecutingTouch;
                    }

                    break;

                case States.ExecutingTouch:
                    break;
            }
        }

        /// <summary>
        /// Called when a manipulation status message is received
        /// </summary>
        /// <param name="message">A manipulation status message</param>
        public void Receive(ManipulatorStatusMessage message)
        {
            this.latestManipulatorStatusMessage = message;
        }
    }
}
